package controllers;

public class ByteHandler {

	/**
	 * 
	 * @param position Is supposed to be between 2306 and 4606.
	 * @return
	 */
	public static byte[] createServoBytes(int position) {
		byte[] bytes = new byte[3];
		String[] byteStrings = new String[3];
		
		String command, valueString;
		
		command = "00011";
		
		valueString = Integer.toBinaryString(position);
		int length = valueString.length();
		if(length >= 13)
			valueString = valueString.substring(length-13, length);
		else
			for(int j = 0; j < 13-length; j++)
				valueString = "0" + valueString;
		
		byteStrings[0] = "00" + valueString.charAt(0) + command;
		byteStrings[1] = "01" + valueString.substring(1,7);
		byteStrings[2] = "10" + valueString.substring(7,13);
		
		SerialTurret.console.addString("Servo:");
		for(int i = 0; i < 3; i++) {
			SerialTurret.console.addString(i + ") " + byteStrings[i]);
			bytes[i] = Integer.valueOf(byteStrings[i], 2).byteValue();
		}
		
		return bytes;
	}

	//OBS steps kan vara negativ, skall returnera stegen den ska ta i byteform
	public static byte[] createStepperMotorBytes(int steps) {
		byte[] bytes = new byte[3];
		String[] byteStrings = new String[3];
		
		String command, valueString;
		
		if(steps < 0)
			command = "00010";
		else
			command = "00001";
		
		valueString = Integer.toBinaryString(Math.abs(steps));
		int length = valueString.length();
		if(length >= 13)
			valueString = valueString.substring(length-13, length);
		else
			for(int j = 0; j < 13-length; j++)
				valueString = "0" + valueString;
		
		byteStrings[0] = "00" + valueString.charAt(0) + command;
		byteStrings[1] = "01" + valueString.substring(1,7);
		byteStrings[2] = "10" + valueString.substring(7,13);
		
		SerialTurret.console.addString("Stepper Motor:");
		for(int i = 0; i < 3; i++) {
			SerialTurret.console.addString(i + ") " + byteStrings[i]);
			bytes[i] = Integer.valueOf(byteStrings[i], 2).byteValue();
		}
		return bytes;
	}

}
